#include "stdafx.h"
#include "Programs\PObH.h"
#include "Utils\PathUtils\HybridGraph.h"

#include "Utils\Polygon_utils\ExtendedPolygon.h"
#include "Utils\ReferencePoint.h"

#include "Utils\UI_utils\Environment.h"
#include "Utils\UI_utils\TimeManager.h"
#include "Utils\UI_utils\Graphics.h"

#include "Utils\Number_utils\AK_conversions_1.h"
#include "Utils\Polygon_utils\PolygonIO.h"


typedef Rational_kernel						K;
typedef Rational_kernel::FT					NT; //number type
typedef Rational_kernel::Point_2			Point;
typedef CGAL::Polygon_2<K>					Polygon;
typedef std::vector<Polygon>				Polygon_vec;
typedef Extended_polygon<K>					Robot_rep;
typedef Reference_point<K>					ReferencePoint;
typedef std::vector<ReferencePoint>			Path;
typedef CGAL::Polygon_set_2<K>				Polygon_set;




void write_path(Path const& path,string filename)
{
	std::ofstream pathFile;
	pathFile.open(filename);
	pathFile << path << endl;
	pathFile.close();
}

void display_scene(int argc, char* argv[])
{
  //typedefs

  //loading files from configuration file
  Time_manager tm;
  tm.write_time_log(std::string("start"));
  
  Environment<> env(argc,argv);
  tm.write_time_log(std::string("set environment"));

  //loading scene from environment
  Polygon_vec&       workspace(env.get_workspace());
  Robot_rep			 my_robot(env.get_robot());	

  //load query
  ReferencePoint q_s (env.get_source_configiration());
  ReferencePoint q_t (env.get_target_configiration());

  global_graphics->draw_polygons<K> (workspace, BLUE);
  my_robot.move_absolute(q_s);
  global_graphics->draw_polygon<K> (my_robot.get_absolute_polygon(), GREEN);
  my_robot.move_absolute(q_t);
  global_graphics->draw_polygon<K> (my_robot.get_absolute_polygon(), RED);

  global_graphics->display();
  return;
}


void single_robot_optimization_example(int argc, char* argv[], int num_path)
{
  //loading files from configuration file
  Time_manager tm;
  tm.write_time_log(std::string("start"));
  
  Environment<> env(argc,argv);
  tm.write_time_log(std::string("set environment"));

  //loading scene from environment
  Polygon_vec&		workspace(env.get_workspace());
  Robot_rep			my_robot(env.get_robot());
   
  //load query
  ReferencePoint q_s (env.get_source_configiration());
  ReferencePoint q_t (env.get_target_configiration());

  global_graphics->draw_polygons<K> (workspace, BLUE);
  my_robot.move_absolute(q_s);
  global_graphics->draw_polygon<K> (my_robot.get_absolute_polygon(), GREEN);
  my_robot.move_absolute(q_t);
  global_graphics->draw_polygon<K> (my_robot.get_absolute_polygon(), RED);

  Input_reader reader;
  H_Graph<K> hgraph(workspace,my_robot.get_original_polygon(),q_s,q_t);
  std::vector<std::vector<ReferencePoint>> paths;

  for (int t=0; t < num_path; t++){
	std::vector<ReferencePoint>	path;
	std::stringstream filename;
	filename << "path" << t << ".txt";
	reader.read_path<K>(filename.str(),path);
	global_graphics->draw_path<K>(path);
	paths.push_back(path);
  }
  global_graphics->display();

  for (int t=0; t < num_path; t++){
	std::vector<ReferencePoint>	path = paths[t];
	hgraph.add_path(path);
  }
  std::vector<ReferencePoint>	opt_path = hgraph.compute_optimal_path();
  write_path(opt_path,"path_opt.txt");
  
  global_graphics->draw_path(opt_path, my_robot, true);
  global_graphics->display();
  return;
}





